using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class TrackJoints : MonoBehaviour {
	[System.Serializable]
	public class Joint
	{
		public string name;
		public Transform transform;
		//public KinectWrapper.Joints joint;
		//public int parentIndex;
		
		private Quaternion initialRotation;
		
		internal bool hasTPose = false;
		internal Vector3 axisUp = Vector3.up;
		internal Vector3 axisForward = Vector3.forward;
		
		public void Awake()
		{
			if (transform != null)
				initialRotation = transform.rotation;
		}
		
		public void Orient(Quaternion worldRotation)
		{
			if (!hasTPose || transform == null)
				return;
			
			Quaternion defaultRotation = Quaternion.LookRotation(axisForward, axisUp);
			
			transform.rotation = worldRotation * Quaternion.Inverse(defaultRotation) * initialRotation;
		}
	}

	public Joint[] joints;
	static KinectWrapper.Joints[] kParents = new KinectWrapper.Joints[] {
		KinectWrapper.Joints.HIP_CENTER,
        KinectWrapper.Joints.HIP_CENTER,
        KinectWrapper.Joints.SPINE,
        KinectWrapper.Joints.SHOULDER_CENTER,
        KinectWrapper.Joints.SHOULDER_CENTER,
        KinectWrapper.Joints.SHOULDER_LEFT,
        KinectWrapper.Joints.ELBOW_LEFT,
        KinectWrapper.Joints.WRIST_LEFT,
        KinectWrapper.Joints.SHOULDER_CENTER,
        KinectWrapper.Joints.SHOULDER_RIGHT,
        KinectWrapper.Joints.ELBOW_RIGHT,
        KinectWrapper.Joints.WRIST_RIGHT,
        KinectWrapper.Joints.HIP_CENTER,
        KinectWrapper.Joints.HIP_LEFT,
        KinectWrapper.Joints.KNEE_LEFT,
        KinectWrapper.Joints.ANKLE_LEFT,
        KinectWrapper.Joints.HIP_CENTER,
        KinectWrapper.Joints.HIP_RIGHT,
        KinectWrapper.Joints.KNEE_RIGHT,
        KinectWrapper.Joints.ANKLE_RIGHT
	};

	static KinectWrapper.Joints[] kRotations = new KinectWrapper.Joints[] {
		KinectWrapper.Joints.SPINE,
        KinectWrapper.Joints.SHOULDER_CENTER,
        KinectWrapper.Joints.HEAD,
        KinectWrapper.Joints.COUNT,
        KinectWrapper.Joints.ELBOW_LEFT,
        KinectWrapper.Joints.WRIST_LEFT,
        KinectWrapper.Joints.HAND_LEFT,
        KinectWrapper.Joints.COUNT,
        KinectWrapper.Joints.ELBOW_RIGHT,
        KinectWrapper.Joints.WRIST_RIGHT,
        KinectWrapper.Joints.HAND_RIGHT,
        KinectWrapper.Joints.COUNT,
        KinectWrapper.Joints.KNEE_LEFT,
        KinectWrapper.Joints.ANKLE_LEFT,
        KinectWrapper.Joints.FOOT_LEFT,
        KinectWrapper.Joints.COUNT,
        KinectWrapper.Joints.KNEE_RIGHT,
        KinectWrapper.Joints.ANKLE_RIGHT,
        KinectWrapper.Joints.FOOT_RIGHT,
        KinectWrapper.Joints.COUNT
	};

	static KinectWrapper.Joints[] kMirrored = new KinectWrapper.Joints[] {
		KinectWrapper.Joints.HIP_CENTER,
        KinectWrapper.Joints.SPINE,
        KinectWrapper.Joints.SHOULDER_CENTER,
        KinectWrapper.Joints.HEAD,
        KinectWrapper.Joints.SHOULDER_RIGHT,
        KinectWrapper.Joints.ELBOW_RIGHT,
        KinectWrapper.Joints.WRIST_RIGHT,
        KinectWrapper.Joints.HAND_RIGHT,
        KinectWrapper.Joints.SHOULDER_LEFT,
        KinectWrapper.Joints.ELBOW_LEFT,
        KinectWrapper.Joints.WRIST_LEFT,
        KinectWrapper.Joints.HAND_LEFT,
        KinectWrapper.Joints.HIP_RIGHT,
        KinectWrapper.Joints.KNEE_RIGHT,
        KinectWrapper.Joints.ANKLE_RIGHT,
        KinectWrapper.Joints.FOOT_RIGHT,
        KinectWrapper.Joints.HIP_LEFT,
        KinectWrapper.Joints.KNEE_LEFT,
        KinectWrapper.Joints.ANKLE_LEFT,
        KinectWrapper.Joints.FOOT_LEFT
	};
#if UNITY_EDITOR
	[UnityEditor.MenuItem("Utils/FillJoints")]
	public static void FillJoints()
	{
		TrackJoints trackJoints = UnityEditor.Selection.activeGameObject.GetComponent<TrackJoints>();

		Joint[] newJoints = new Joint[(int)KinectWrapper.Joints.COUNT];
		for (int i = 0; i < newJoints.Length; ++i)	
		{
			newJoints[i] = new Joint();
			//newJoints[i].joint = (KinectWrapper.Joints)i;
			newJoints[i].name = ((KinectWrapper.Joints)i).ToString();
			//newJoints[i].parentIndex = (int)kParents[i];
			//if (newJoints[i].parentIndex == i)
			//	newJoints[i].parentIndex = -1;

			if (i < trackJoints.joints.Length)
				newJoints[i].transform = trackJoints.joints[i].transform;
		}

		trackJoints.joints = newJoints;
	}
#endif

	public Transform backgroundPlane;
    public bool mirrored;
    public bool headAndEyesOnly;
    public Vector3 scale = new Vector3(0.001f, 0.001f, 0.001f);

   	KUInterface kinectInterface;

   	Vector3 BodyToTorsoDelta;

//*********************************************************************
// ******************** MAIN FUNCTIONS (MONO BEHAVIOUR) ***************
//*********************************************************************

	void Awake()
	{
		foreach(Joint j in joints)
			j.Awake();
		
		AssingKinectTPose(KinectWrapper.Joints.HIP_CENTER, 		new Vector3(0, 0.7f, 0.8f), new Vector3(0, 0.8f, -0.6f));
		AssingKinectTPose(KinectWrapper.Joints.SPINE, 			new Vector3(0, 1, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.SHOULDER_CENTER, new Vector3(0, 1, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.SHOULDER_LEFT, 	new Vector3(-1, 0, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.ELBOW_LEFT, 		new Vector3(-1, 0, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.WRIST_LEFT, 		new Vector3(-1, 0, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.SHOULDER_RIGHT, 	new Vector3(1, 0, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.ELBOW_RIGHT, 	new Vector3(1, 0, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.WRIST_RIGHT, 	new Vector3(1, 0, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.HIP_LEFT, 		new Vector3(0, -1, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.KNEE_LEFT, 		new Vector3(0, -1, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.ANKLE_LEFT, 		new Vector3(0, 1, 0), new Vector3(0, 0, 1));
		AssingKinectTPose(KinectWrapper.Joints.HIP_RIGHT, 		new Vector3(0, -1, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.KNEE_RIGHT, 		new Vector3(0, -1, 0), new Vector3(0, 0, -1));
		AssingKinectTPose(KinectWrapper.Joints.ANKLE_RIGHT, 	new Vector3(0, 1, 0), new Vector3(0, 0, 1));
	}
	
	void AssingKinectTPose(KinectWrapper.Joints j, Vector3 axisUp, Vector3 axisForward)
	{
		axisUp.Normalize();
		axisForward.Normalize();
		
		if (mirrored)
		{
			axisUp.x = -axisUp.x;
			axisForward.x = -axisForward.x;
		}
		
		int index = (int)j;
		
		Joint joint = joints[index];
		joint.axisUp = axisUp;
		joint.axisForward = axisForward;
		joint.hasTPose = true;
	}

	void Start () 
	{
		kinectInterface = GameObject.FindObjectOfType(typeof(KUInterface)) as KUInterface;
	}
	
	void Update () 
	{

		UpdateSkeleton();

		//Quaternion n = kinectInterface.GetJointRotation(1, KinectWrapper.Joints.ELBOW_LEFT);
		//Debug.Log(n);
	}

//*********************************************************************
// ******************** UPDATE FUNCTIONS ******************************
//*********************************************************************

	void UpdateSkeleton()
	{
		
		Quaternion[] worldRot = new Quaternion[(int)KinectWrapper.Joints.COUNT];
		Vector3[] worldPos = new Vector3[worldRot.Length];
		
		foreach (KinectWrapper.Joints j in System.Enum.GetValues(typeof(KinectWrapper.Joints)))
		{
			if (j == KinectWrapper.Joints.COUNT)
				continue;

			int index = (int)j;
			int parentIndex = (int)kParents[index];

			Quaternion rot = kinectInterface.GetJointRotation(j);

			worldRot[index] = parentIndex == index ? rot :  worldRot[parentIndex] * rot;
			worldPos[index] = kinectInterface.GetJointPos(j);

			//if (j == KinectWrapper.Joints.SHOULDER_RIGHT)
			//	Debug.Log("\t" + worldRot[index].eulerAngles.x + "\t" + worldRot[index].eulerAngles.y + "\t" + worldRot[index].eulerAngles.z);
		}
		/*
		if (headAndEyesOnly)
		{
			headScript.target = worldPos[0];
			headScript.target.z *= -1;
			//headScript.target.z -= 1000;
			return;
		}
		else */
		{
			//Debug.Log("Len: " + joints.Length);
			//Debug.Log("worldPos: " + worldPos[0]);
			joints[0].transform.position = Vector3.zero;//worldPos[0];
		}
		
		foreach (KinectWrapper.Joints j in System.Enum.GetValues(typeof(KinectWrapper.Joints)))
		{
			if (j == KinectWrapper.Joints.COUNT)
				continue;

			int index = (int)j;
			int parentIndex = (int)kParents[index];

			Vector3 v0 = worldPos[index];
			Vector3 v1 = worldPos[parentIndex];
			Debug.DrawLine(v0, v1);
			
			float scale = 75f;
			
			KinectWrapper.Joints rj = kRotations[index];
			if (rj != KinectWrapper.Joints.COUNT)
			{
				Quaternion rot = worldRot[(int)rj];

				Debug.DrawRay(v0, rot * Vector3.right * scale, Color.red);
				Debug.DrawRay(v0, rot * Vector3.up * scale, Color.green);
				Debug.DrawRay(v0, rot * Vector3.forward * scale, Color.blue);

				int jointIndex = (int)(mirrored ? kMirrored[index] : j);
				Joint joint = joints[jointIndex];	
				joint.Orient(rot);
				
				if (joint.transform != null)
				{
					Vector3 p = joint.transform.position;
					Debug.DrawRay(p, joint.axisUp * scale, Color.green * 0.7f);
					Debug.DrawRay(p, joint.axisForward * scale, Color.blue * 0.7f);
				}
			}			
		}
	}
}
